#include<iostream>
using namespace std;
#include<cmath>

/*
 * GaussLinear: solves n-dim linear equations
 * The solution is stored in x
 * Permutation of rows is activated, when the principle element is smaller than min
 * If A is singular, a permutation will fail and function exit(1), signals error
 */
void GaussLinear(int n, double **A, double *x, double min){

	int i,j,k;
	double y;
	for(i=0;i<n;i++){
		if( fabs( A[i][i] ) < min ){// permutation is activated
			for(j=i+1;j<n;j++){
				if( fabs( A[j][i] ) > min ){
					for(k=i;k<n;k++){
						y = A[i][k];
						A[i][k] = A[j][k];
						A[j][k] = y;
					}
					break;
				}
			}
			if(j==n){// didn't find any element larger than min
				cerr<<" A is singular in GaussLinear(n,A,x,min).\n";
				exit(1);
			}
		}
		for(j=i+1;j<n;j++){// row operations
			y = - A[j][i] / A[i][i];
			for(k=0;k<n+1;k++){
				A[j][k] = y * A[i][k] + A[j][k];
			}
		}
		// for test only
		/*
		cout<<"i="<<i<<"\t A:"<<endl;
		for(j=0;j<n;j++){
			for(k=0;k<n+1;k++){
				cout<<A[j][k]<<",";
			}
			cout<<endl;
		}
		*/
	}
	for(j=n-1;j>=0;j--){
		y=0;
		for(k=j+1;k<n;k++) y += A[j][k]*x[k];
		x[j] = 1/A[j][j] * ( A[j][n] - y );
	}
}

double LeastSquare(int N, double **x_data, double *y_data, int n, int m, double *a, double (**func)(int, double *), double precision ){

	int l, i, j;
	double y,z;
	double ** A = new double * [ m ];
	for(l=0;l<m;l++){
		A[l] = new double [m+1];
		for(i=0;i<m;i++){
			y=0;
			for(j=0;j<N;j++){
				y += func[l](n, x_data[j]) * func[i](n, x_data[j]);
			}
			A[l][i] = y;
		}
		y=0;
		for(j=0;j<N;j++){
			y += func[l](n, x_data[j]) * y_data[j];
		}
		A[l][i] = y;
	}
	// for test only
	/*
	cout<<"A is ready"<<endl;
	for(i=0;i<m;i++){
		for(j=0;j<m+1;j++){
			cout<<A[i][j]<<",";
		}
		cout<<endl;
	}
	*/
	GaussLinear(m, A, a, precision);
	y=0;
	for(j=0;j<N;j++){
		z=0;
		for(i=0;i<m;i++)
			z += a[i] * func[i](n, x_data[j]);
		z -= y_data[j];
		y += z*z;
	}
	y = sqrt( y/N );
	for(i=0;i<m;i++) delete [] A[i];
	delete [] A;
	return y;// returns root-mean-square-root
}

double g0(int n, double *x){
	return 1;
}

double g1(int n, double *x){
	double z = 0;
	for(int i=0;i<n;i++) z += x[i];
	return z;
}

double g2(int n, double *x){
	double z = 0;
	for(int i=0;i<n;i++) z += x[i] * x[i];
	return z;
}

double g3(int n, double *x){
	double z = 0;
	for(int i=0;i<n;i++) z += pow(x[i],3);
	return z;
}

int main(){

	int i,j,n=1, m=3,N=10;
	double ( *g[3] )(int, double * )={g0,g1,g2};

	double **x_data = new double * [N];
	for(i=0;i<N;i++) x_data[i] = new double [n];
	double *y_data = new double [N];

	for(i=0;i<N;i++){
		x_data[i][0]=i;
		y_data[i]=1.234+2.456*i+3.789*i*i;
	}

	double *a = new double [m];
	cout<<"RMSD="<<LeastSquare(N, x_data, y_data, n, m, a, g, 0.01 )<<endl;
	cout<<"a: "; for(i=0;i<m;i++)cout<<a[i]<<","; cout<<endl;

	delete [] a, y_data;
	for(i=0;i<N;i++)delete [] x_data[i]; delete [] x_data;

	return 0;
}
